
#include "CSEREPDataExchanger.h"

using namespace SORT::DataIO;
using namespace SORT::ObjectRecognition;


CSEREPDataExchanger::CSEREPDataExchanger()
{
	m_coms.run();
}

void CSEREPDataExchanger::addInstances( mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes )
{
	string	str;
	string	str_response;

	str.append( ADD_INSTANCES );
	
	size_t N_planes	= v_planes.size();

	for ( size_t i = 0; i < N_planes; i++ )
	{
		str.append( ELEMENT_SEPARATOR );

		str.append( "type=plane" );
		str.append( FIELD_SEPARATOR );

		// Store the plane id
		str.append( format("id=%d",v_planes[i].getId() ) );
		str.append( FIELD_SEPARATOR );

		// Store the orientation
		string type;
		if ( v_planes[i].m_type == CPlane::TPlaneType::VERTICAL )
			type = "VERTICAL";
		else if (v_planes[i].m_type == CPlane::TPlaneType::ALMOST_VERTICAL )
			type = "ALMOST_VERTICAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::HORIZONTAL )
			type = "HORIZONTAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::ALMOST_HORIZONTAL )
			type = "ALMOST_HORIZONTAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::DIAGONAL )
			type = "DIAGONAL";

		str.append( format("orientation=%s", type.c_str() ) );
		str.append( FIELD_SEPARATOR );

		// Store coordinates in the MRPT coordinate' system
		//                 
		//			   z ^	   ^
		//				 |    / x
		//				 |   /
		//				 |  /
		//				 | /
		//   y <---------|/
		//		
		str.append( format("centroid_x=%f", v_planes[i].m_centroid[0]) ); // has_centroid_x
		str.append( FIELD_SEPARATOR );
		str.append( format("centroid_y=%f", v_planes[i].m_centroid[1]) ); // has_centroid_y
		str.append( FIELD_SEPARATOR );
		str.append( format("centroid_z=%f", v_planes[i].m_centroid[2]) ); // has_centroid_z
		str.append( FIELD_SEPARATOR );

		str.append( format("area=%f", v_planes[i].getHeight() * v_planes[i].getWidth() ) );
		str.append( FIELD_SEPARATOR );

		str.append( format("elongation=%f", v_planes[i].getElongation() ) );
	}


	if (!transmit(str) )
		cout << "Error sending addInstances command" << endl;

	if (!receive(str_response))
		cout << "Error receiving addInstances command response" << endl;

	if ( str_response == "error" )
		cout << "Error executing the addInstances command in the server" << endl;
}

void CSEREPDataExchanger::getClassificationResults( std::string &res )
{
	string	str;

	str.append( GET_CLASSIFICATION_RESULTS );
	
	if (!transmit(str) )
		cout << "Error sending getClassificationResults command" << endl;

	if ( !receive(res) )
		cout << "Error receiving getClassificationResults command response" << endl;

	if ( res == "error" )
		cout << "Error executing the getClassificationResults command in the server" << endl;
}

//-----------------------------------------------------------
//					     transmit
//-----------------------------------------------------------

bool CSEREPDataExchanger::transmit( string &command ) 
{
	// '|' character is used as command delimiter
	//command.append("|");
	command.append("!");

	size_t written = m_coms.write( command );

	if (!written){
		return false;
	}

	//cout << command << endl;
	//cout << "Sended: " << command << endl;

	return true;

}

//-----------------------------------------------------------
//					     receive
//-----------------------------------------------------------

bool CSEREPDataExchanger::receive(string &response) 
{

	// Read response
	if (!m_coms.read( response ))
		return false;	

	//cout << endl << "Response: " << response << endl;

	return true;	

}